Potential Fields in 3 Joint Space

By Brian Lesko, Morgan Strauss
Graduate Researcher and Teaching Associate, Masters of mechanical engineering student
For Robotics Project | Robotics 7752 | 11/29/2022

Introduction

This live script characterizes the development journey through a path planning algorithm reffered to as the potential fields method - authored by Brian Lesko in collaboration with Morgan Strauss.
The potential field method is applied here in the 3 joint space for an RRP robot having its revolute joints overlap at the origin. This architecture exists in the robot produced by Intuitive Surgical known as the dVRK. It is used by surgeons to teleoperate, often, multiple of these robots are used all with end effector control separate from the positional control demonstrated in this script.

Motivation

There are multiple different existing path planning algorithms that exist which could potentially be used to solve this problem. One of these algorithms is PRM, in which a roadmap of the free space of the robot is created and an efficient path through this space is solved using a searching algorithm such as A*. The upside to this option is that once the roadmap is created an optimal path can be quickly built around any obstacles. The drawbacks are that a representation of the available free space must be created, and there are multiple options as to how to sample the joint space and detect collision points. Another option that was considered was to use RRT*, or a variation of the rapidly exploring random trees method which continuously improves its initial solution to find a time or distance efficient path. This method will improve the solution over time and approach an optimally short path, but it takes multiple attempted paths to reach a solution which can be more computationally intensive than our chosen method.
The path planning algorithm that was chosen for this project was the virtual potential fields method. This method was chosen due to it being more intuitive and for the ability to check the accuracy of the model through plots of the potentials and gradients that were created. The main downside to the virtual potential fields’ method is that you can get trapped in local minima. To prevent this, the strength of the potential fields and gradients for the goal and the obstacles can be altered to prevent the algorithm from falling into these local minimums

Start of Script

clc; clear; close all;

AKA Stanford Robot Architecture

[Slist,Mlist_joints] = getdVRKArchitecture();

A Robot Configuration, described

The robot configuration in the task space can be described as a list of occupied points, these points will then be used later to create an obstacle potential field
% Forward Kinematics to find all joint locations
theta = [pi/3 pi/3 .5]';
jointLocations = getJointLocations(Slist(:,1:3),Mlist_joints,theta);
 
% add n points lying between each joint on their respective link
n = 3; % Test Points per Link
configurationPoints = getConfigurationPoints(jointLocations,n);
 
% the 1st two joints of the dVRK are overlapping, throw out points from the "link" between them
configurationPoints = configurationPoints(3:length(configurationPoints(:,1)),:);

Get potential of configuration with respect to an obstacle

The obstacle potential over the joint space can be found using a sum of distances between points along the prismatic link on the robot and an obstacle surface
obstacle = [.04 .04 -.1 .02];
potential = getObstaclePotentialAtConfigurationPoints(configurationPoints,obstacle)
potential = 178.2492
The obstacle and the points along the primsatic link of the robot on the joint space can then be plotted.
plotMesh(configurationPoints); hold on
sphereMesh = getSphereMesh(obstacle(1:3)',obstacle(4),10);
plotMesh(sphereMesh); hold off

Repeat for all Obstacles, sum

The obstacle potential for each obstacle can then summed to get an overall obstacle potential.
obstacles = [[1 1 1 0];[2 2 2 0]];
potentials = zeros(length(obstacles(:,1)),1);
for i = 1:1:length(obstacles(:,1))
potentials(i) = getObstaclePotentialAtConfigurationPoints(configurationPoints,obstacles(i,:));
end
potential = sum(potentials)
potential = 2.1074

Unobstructed Configuration Space of the Robot

To describe the entire configuration space of the robot, a number of gridpoints is set for each angle. The joint space is then described by gridded points, specific for RRP, with joint limits as the end points of the grid.
nGridPoints = 6;
jointMaxes = [[50 50]*pi/180 .15];
jointMins = [[-50 -50]*pi/180 .028];
thetas = getCSpacePermutations(nGridPoints,jointMaxes,jointMins);

Define a Goal Position in the joint space

A goal position in the joint space and task space is then created by calculating the squared distance from every configuration to the goal position.
M3 = Mlist_joints(:,:,1)*Mlist_joints(:,:,2)*Mlist_joints(:,:,3);
startTheta = [0 0 .03]';
goalTheta = [[40 40]*pi/180 .12]';

Goal Potential Field

The goal position in the joint space is used to find the goal potential over the entire joint space, which is then plotted.
goalPotentialField = getGoalPotentialField(goalTheta,nGridPoints); plotStartEnd(startTheta,goalTheta)

A Highly Gridded Potential Field

Kevin.jpg

Goal Gradient Field

Using the equation from the textbook, the goal gradient is then calculated and then plotted.
goalGradientField = getGoalGradient(goalTheta,goalPotentialField); plotStartEnd(startTheta,goalTheta)

A Gradient Field with a low number of gridpoints

arrows.jpg

The Obstacle Potential Field

This uses the same methods as before to calculate an overall obstacle potential, and plot it.
obstacles = [1.837 -0.399 -126.255 20]/1000; %a ball near the origin
obstaclesTheta = [0 0.02 0.1328] % its position in theta space
obstaclesTheta = 1×3
0 0.0200 0.1328
thetaMesh = thetas;
obstaclePotentialField = getObstaclePotentialField(Slist(:,1:3),Mlist_joints,thetaMesh,obstacles);
obP = obstaclePotentialField;
scatter3(obP(:,1),obP(:,2),obP(:,3),[],obP(:,4),'o'),colorbar
title('Obstacle Potential Field')
view([44.619 35.189])

Obstacle Gradient Field

Using the textbook equations, the obstacle gradient can also be calculated for every configuration.
obstacleGradientField = getObstacleGradientField(Slist(:,1:3),Mlist_joints,thetaMesh,obstacles,obstaclesTheta); plotStartEnd(startTheta,goalTheta)

Combining the Fields

This section is used to change the weights of each potential field and add them together
gradientField = obstacleGradientField;
totalScale = 1/40;
for i = 4:1:7
gradientField(:,i) = totalScale*(.05*obstacleGradientField(:,i) + 3*goalGradientField(:,i));
end
 
gf = gradientField;
quiver3(gf(:,1),gf(:,2),gf(:,3),gf(:,5),gf(:,6),gf(:,7),'b')
view([-29.671 -33.270]), title('Total Potential Field')
plotStartEnd(startTheta,goalTheta)
xlabel('\theta_1'), ylabel('\theta_2'), zlabel('\theta_3')
totalGradient.jpg

Creating a Path through the Gradient Field

Using the control law and the calculated gradients, a trajectory to the goal position can be found:
dt = 1/10;
trajectory = getPathThroughGradientField(startTheta,gradientField,goalTheta,dt)
lastTheta = trajectory(length(trajectory(:,1)),:)
T = FKinSpace(M3, Slist(:,1:3), lastTheta'); goalPositionFromTrajectory = T(1:3,4)'

Trajectory Through the Field

figure
P = goalPotentialField;
scatter3(P(:,1),P(:,2),P(:,3),[],P(:,4),'o'),colorbar
% quiver3(gf(:,1),gf(:,2),gf(:,3),gf(:,5),gf(:,6),gf(:,7),'b')
hold on
plot3(startTheta(1),startTheta(2),startTheta(3),'ko')
plot3(goalTheta(1),goalTheta(2),goalTheta(3),'ro')
plot3(trajectory(:,1),trajectory(:,2),trajectory(:,3),'--m')
hold off
Trajectory.jpg
The Goal Position is reached by this joint space trajectory
goalPositionFromTrajectory

Animating the Produced Motion

timeTotal = 20;
filename = 'gifBABY.gif';
pointMesh = getSphereMesh(obstacles(1,1:3)',obstacles(1,4),10);
animateRobot4(trajectory,timeTotal,Slist,Mlist_joints,filename,pointMesh)

Potential Fields Animation

gifBABY.gif
The Potential Field and Associated Gradient are followed until the goal position in this trajectory, while avoiding the spherical obstacle.
In the future, the potential for the prismatic joint should be reduced.

Hardware Demonstration

The trajectory generation code shown above was used to create a path through the joint space and around an obstacle. The output of this code was a .mat file named Trajectory, which can be loaded with our move_trajectory function. This function was used to move the dVRK through a generated path, as shown in the video below:
% To Command the actual Robot
% function [commanded_joints_all, measured_all,goal_all] = move_trajectory()

Summary:

To solve for the trajectory, the obstacle and goal potentials were both found in the joint space using the equations for each potential in the textbook. The gradient for the obstacle and goal could then be found in 3D space over each potential. Both gradients and potentials can then be added to give a total gradient and potential across the joint space. The overall gradient was then moved through beginning at the start position until the desired goal position was reached. This was then saved as a trajectory in the joint space, which was passed to the dVRK to cause it to move through the calculated path.

Functions

New Functions

Obstacle Gradient
function obstacleGradient = getObstacleGradientField(Slist,Mlist_joints,thetaMesh,obstacles,obstaclesTheta)
obstacleGradient = thetaMesh;
for i = 1:1:length(thetaMesh(:,1))
theta = thetaMesh(i,1:3)';
jointLocations = getJointLocations(Slist(:,1:3),Mlist_joints,theta);
n = 4; % Test Points per Link
configurationPoints = getConfigurationPoints(jointLocations,n);
% because this is the dVRK architecture, there is no link between R, R
configurationPoints = configurationPoints(3:length(configurationPoints(:,1)),:);
 
potentials = zeros(length(obstacles(:,1)),1);
vector = zeros(length(obstacles(:,1)),3);
for j = 1:1:length(obstacles(:,1))
potentials(j) = getObstaclePotentialAtConfigurationPoints(configurationPoints,obstacles(j,:));
% For a vector between the obstacle and the halfway point on the robot
vectorEnd = [theta(1:2)', theta(3)/2];
vectorStart = obstaclesTheta(j,:);
vector(j,:) = vectorEnd-vectorStart; % Where this vector should define the direction of the gradient field vectors
end
% Setting the Obstacle Gradient and Potential at each row
obstacleGradient(i,4) = sum(potentials);
p = sum(potentials);
obstacleGradient(i,5) = sum(vector(:,1)); obstacleGradient(i,6) = sum(vector(:,2)); obstacleGradient(i,7) = sum(vector(:,3));
% Normalizing the Gradients then multiplying by the potential
normal = norm(vector(j,:));% norm([obstacleGradient(i,5),obstacleGradient(i,6),obstacleGradient(i,7)]);
obstacleGradient(i,5) = obstacleGradient(i,5)/normal^4;
obstacleGradient(i,6) = obstacleGradient(i,6)/normal^4;
obstacleGradient(i,7) = obstacleGradient(i,7)/(2*normal)^4;
end
 
% Normalized from 0 to 1
obstacleGradient(:,4) = obstacleGradient(:,4)-min(obstacleGradient(:,4)); % force min to 0
obstacleGradient(:,4) = obstacleGradient(:,4)/max(obstacleGradient(:,4)); %normalized potentials
 
% Multiply the Gradient Directions by the normalized field strength
for i = 1:1:length(obstacleGradient(:,2))
for j = 5:1:7
obstacleGradient(i,j) = obstacleGradient(i,j);%*obstacleGradient(i,4);
end
end
 
gf = obstacleGradient;
quiver3(gf(:,1),gf(:,2),gf(:,3),gf(:,5),gf(:,6),gf(:,7),'b')
end
Goal Gradient
function goalGradient = getGoalGradient(goalTheta,thetas)
goalGradient = thetas;
for i = 1:1:length(thetas(:,1))
goalGradient(i,5) = goalTheta(1)' - thetas(i,1);
goalGradient(i,6) = goalTheta(2)' - thetas(i,2);
goalGradient(i,7) = goalTheta(3)' - thetas(i,3);
end
gf = goalGradient;
quiver3(gf(:,1),gf(:,2),gf(:,3),gf(:,5),gf(:,6),gf(:,7),'b')
end
Get Neighboring Potentials
function [neighborP] = neighborPotentials(field,row,n)
P_curr = field(row,4);
for i =-1:2:1 %Go forward and backward in x:
currentRow = row+(n^2*i);
if currentRow < 1 || currentRow > n^3
P_new = 1.25*P_curr;
else
P_new = field(currentRow,4);
end
neighborP(0.5*i+1.5,1) = P_new;
end
for i =-1:2:1 %Go forward and backward in y:
currentRow = row+n*i;
if currentRow < 1 || currentRow > n^3
P_new = 1.25*P_curr;
else
P_new = field(currentRow,4);
end
neighborP(0.5*i+1.5,2) = P_new;
end
for i =-1:2:1 %Go forward and backward in z:
currentRow = row+i;
if currentRow < 1 || currentRow > n^3
P_new = 1.25*P_curr;
else
P_new = field(currentRow,4);
end
neighborP(0.5*i+1.5,3) = P_new;
end
end
Plot the starting and ending values
function plotStartEnd(startTheta,goalTheta)
hold on
plot3(startTheta(1),startTheta(2),startTheta(3),'o','MarkerFaceColor','k')
plot3(goalTheta(1),goalTheta(2),goalTheta(3),'o','MarkerFaceColor','r')
hold off
end
make a Gradient Field
function gradientField = getGradientField(potentialField,scale)
 
nGridPoints = ceil(length(potentialField(:,1))^(1/3));
size = [nGridPoints,nGridPoints,nGridPoints];
cube = reshape(potentialField(:,4),size);
 
[fx,fy,fz] = gradient(cube);
size = [nGridPoints^3 1];
fx = -reshape(fx,size);fy = -reshape(fy,size);fz = -reshape(fz,size);
fx = fx/max(fx); fy = fy/max(fy); fz = fz/max(fz); %normalize the Gradient
fx = fx*scale(1); fy = fy*scale(2); fz = fz*scale(3);
weight = 1*potentialField(:,4);
gradientField = [potentialField(:,1:4),weight.*fx,weight.*fy,weight.*fz];
gf = gradientField;
quiver3(gf(:,1),gf(:,2),gf(:,3),gf(:,5),gf(:,6),gf(:,7),'r')
end
 
function dataNew = getGradientFieldAI(data)
 
% Set the step sizes for the numerical gradient calculation
dx = 0.001;
dy = 0.001;
dz = 0.001;
 
n = ceil(length(data(:,1))^(1/3))
% Pre-allocate space for the gradient vectors
dVdx = zeros(size(n));
dVdy = zeros(size(n));
dVdz = zeros(size(n));
 
% Loop through each point in the data
for i = 1:length(n)
 
[np] = neighborPotentials(data,i,n); %negative in the top
% Use the nearby potentials to approximate the partial derivatives
dVdx(i) = (np(2,1) - np(1,1)) / (2 * dx);
dVdy(i) = (np(2,2) - np(1,2)) / (2 * dy);
dVdz(i) = (np(2,3) - np(1,3)) / (2 * dz);
end
 
% The gradient at each point is the vector (dVdx, dVdy, dVdz)
 
dataNew = data;
dataNew(:,5) = dVdx;
dataNew(:,6) = dVdy;
dataNew(:,7) = dVdz;
% The gradient at each point is the vector (dVdx, dVdy, dVdz)
end
Path Through Gradient Field
function trajectory = getPathThroughGradientField(startTheta,gradientField,goalTheta,dt)
% By Morgan Strauss 12/11/22
% Adapted by Brian Lesko 12/11/22
%Inputs: p - starting position (js), gradient - gradient that you want to follow
%Outputs: trajectory - overall trajectory after following gradient
i =1;
e = 0.02;
while(norm(startTheta-goalTheta)>e && i < 150) %Set precision based on theta3 bounds
row = findClosest(startTheta,gradientField);
if abs(startTheta(1)) > max(gradientField(:,1)) || abs(startTheta(2)) > max(gradientField(:,2)) || startTheta(3) > max(gradientField(:,3)) || startTheta(3) < min(gradientField(:,3))
disp('Trajectory lead out of bounds')
startTheta = gradientField(row,1:3)';
end
trajectory(i,:) = startTheta';
startTheta = startTheta + dt*gradientField(row,5:7)';
i = i+1;
end
save('Trajectory.mat','trajectory')
end
 
function row = findClosest(p,gradient)
tol = 10000;
for i = 1:length(gradient(:,1))
if norm(gradient(i,1:3)-p) < tol
row = i;
tol = norm(gradient(i,1:3)-p);
end
end
end
Get Goal Potential Field
function [goalP] = getGoalPotentialField(goalTheta,n)
%%Potential field calculation
% Tabulate potential field in a gridded joint space
theta_1_test = linspace(-50,50,n)*pi/180;
theta_2_test = linspace(-50,50,n)*pi/180;
theta_3_test = linspace(0.028,0.15,n);
 
theta1Range = max(theta_1_test)-min(theta_1_test);
theta2Range = max(theta_2_test)-min(theta_2_test);
theta3Range = max(theta_3_test)-min(theta_3_test);
%Assign Obstacle Potential Scale:
goalP = [];
 
% check for collisions along the length of the manipulator
for i = 1:length(theta_1_test)
for j = 1:length(theta_2_test)
for h = 1:length(theta_3_test)
% goal field is the minimum squared distance to the nearest inverse
% kinematics solution
theta = [theta_1_test(i); theta_2_test(j); theta_3_test(h)];
 
P = 100*norm( (theta-goalTheta') ./[theta1Range; theta2Range; theta3Range] )^2;
 
%Put all output values together:
rowP = [theta_1_test(i) theta_2_test(j) theta_3_test(h) P i j h];
goalP = [goalP; rowP];
end
end
end
% goalP(:,4) = goalP(:,4)-min(goalP(:,4)); % force the min to zero
% goalP(:,4) = goalP(:,4)./max(goalP(:,4)); %normalize the max to zero
figure()
scatter3(goalP(:,1),goalP(:,2),goalP(:,3),[],goalP(:,4)./max(goalP(:,4)),'o','MarkerFaceAlpha',0.2);
xlabel('\Theta_1'), ylabel('\Theta_2'), zlabel('\Theta_3'), title('Goal Potential Field')
colorbar
end
Combine Obstacle Potential Fields Work
function obstaclePotentialField = getObstaclePotentialField(Slist,Mlist_joints,thetaMesh,obstacles)
obstaclePotentialField = thetaMesh;
for i = 1:1:length(thetaMesh(:,1))
theta = thetaMesh(i,1:3)';
jointLocations = getJointLocations(Slist(:,1:3),Mlist_joints,theta);
n = 3; % Test Points per Link
configurationPoints = getConfigurationPoints(jointLocations,n);
% because this is the dVRK architecture, there is no link between R, R
configurationPoints = configurationPoints(3:length(configurationPoints(:,1)),:);
 
potentials = zeros(length(obstacles(:,1)),1);
for j = 1:1:length(obstacles(:,1))
potentials(j) = getObstaclePotentialAtConfigurationPoints(configurationPoints,obstacles(j,:));
end
obstaclePotentialField(i,4) = sum(potentials);
end
% Normalized to 1
% obstaclePotentialField(:,4) = obstaclePotentialField(:,4)-min(obstaclePotentialField(:,4)); % force min to 0
% obstaclePotentialField(:,4) = obstaclePotentialField(:,4)/max(obstaclePotentialField(:,4)); %normalize
end
C Space joint Permutations list with indicies for searching
function thetas = getCSpacePermutations(gridPoints,jointMaxes,jointMins)
% Making Theta Vectors
jointNum = length(jointMaxes(1,:));
thetaVecs = zeros(gridPoints,jointNum);
for j = 1:1:jointNum
thetaVecs(:,j) = linspace(jointMins(j),jointMaxes(j),gridPoints);
end
 
thetas =[];
for i = 1:1:gridPoints
for j = 1:1:gridPoints
for k = 1:1:gridPoints
theta = [thetaVecs(i,1) thetaVecs(j,2) thetaVecs(k,3)];
thetaIndicies = [theta,[0,i,j,k]];
thetas = [thetas; thetaIndicies];
end
end
end
end
get Obstacle Potential Field
function potential = getObstaclePotentialAtConfigurationPoints(configurationPoints,obstacle)
numTestLocations = length(configurationPoints(:,1));
distances = zeros(numTestLocations,1);
for i = 1:1:numTestLocations
testLocation = configurationPoints(i,:);
obstacleLocation = obstacle(1:3);
radius = obstacle(4);
distances(i) = norm(testLocation-obstacleLocation) - radius;
end
potential = sum(1./distances.^2);
end
Get Joint Locations
function jointLocations = getJointLocations(Slist,Mlist_joints,theta)
jointLocations = zeros(length(Slist(1,:)),3);
for n = 1:1:length(Slist(1,:))
T = customForwardKinematics(Mlist_joints,Slist,n,theta);
[~,p] = TransToRp(T); %the nth joint position at t is p
jointLocations(n,:) = p;
end
end
get Configuration Points of a robot, joint locations and n sampled Points on each link
function pointMesh = getConfigurationPoints(jointLocations,n)
linkNumber = length(jointLocations(:,1))-1;
for i = 1:1:linkNumber %for each link
currentJointLocation = jointLocations(i,:);
nextJointLocation = jointLocations(i+1,:);
vector = currentJointLocation-nextJointLocation;
for m = 0:1:n
pointMesh(i+m+1,:) = currentJointLocation + m*vector/(n+1);
end
end
% The End effector location
pointMesh = [pointMesh;jointLocations(length(jointLocations(:,1)),:)];
end

To Move the dVRK

function [commanded_joints_all, measured_all,goal_all] = move_trajectory()
%Function to zero Dvrk then move it through path
%Inputs: p, path of first three joints of dvrk (each joint is a row)
%Outputs: commanded and measured join positions throughout movement
load Trajectory.mat trajectory
r = dvrk.arm('PSM1');
t1 = trajectory(:,1); %Joint three angles in degrees (converted to radians below)
t2 = trajectory(:,2); %Joint two angles in degrees (converted to radians below)
d3 = trajectory(:,3); % Joint three distances in meters
measured_all = zeros(4,4,size(t1,2));
commanded_joints_all = zeros(6,1,size(t1,2));
% general settings
rate = 200; % aiming for 200 Hz
ros_rate = rosrate(rate);
 
for j=1:length(t1)
if abs(t1(j)) < deg2rad(51) && abs(t2(j)) < deg2rad(51) && abs(d3(j)) < 0.15
goal = [t1(j);t2(j);d3(j);0;0;0];
end
goal_all(:,j) = goal;
% goal = [deg2rad(m(j));deg2rad(k(j));0.07;0;0;0];
r.move_jp(goal).wait();
commanded_joints_all(:,:,j) = r.measured_js()
measured_all(:,:,j) = r.measured_cp()
end
for j=length(t1):-1:1
if abs(t1(j)) < deg2rad(51) && abs(t2(j)) < deg2rad(51) && abs(d3(j)) < 0.15
goal = [t1(j);t2(j);d3(j);0;0;0];
end
goal_all(:,j) = goal;
% goal = [deg2rad(m(j));deg2rad(k(j));0.07;0;0;0];
r.move_jp(goal).wait();
commanded_joints_all(:,:,j) = r.measured_js()
measured_all(:,:,j) = r.measured_cp()
end
 
end

Past Work Functions

function drawAxis(T,darkness,size)
% Original Author: Brian Lesko 11/24/22
% Revised 11/30/22 by Brian Lesko
 
%Axis Colors
f = 1+darkness; %Darkness should be from 0 to 1
color = [ [182, 2, 8]/182/f ; [59, 114, 29]/114/f ; [4, 110, 143]/143/f ];
 
location = T(1:3,4);
orientation = T(1:3,1:3);
 
%plotting 3 vectors
for i = 1:1:3
vec = orientation(:,i)*size;
quiver3(location(1),location(2),location(3),vec(1),vec(2),vec(3),'LineWidth',2,'Color',color(i,:));
end
end
function Torthogonal = getTOrthogonal(origin)
z = (origin/norm(origin));
x = cross([0 0 1]',z);
x = x/norm(x);
y = cross(z,x);
y = y/norm(y);
orientation = [x y z];
Torthogonal = [orientation,origin ; [0 0 0 1]];
end
Analytical IK
function thetaMesh = analyticalInverseKinematicsRRP(M,Slist,mesh)
% mesh is a list of points x,y,z nx3
% thetaMesh is the configuration space corresponding to each point
% Theta 3 Prismatic Offset
pOffset = .013; % mm
thetaMesh = zeros(length(mesh(:,1)),3);
for i = 1:1:length(mesh(:,1))
p = mesh(i,:)';
T = getTOrthogonal(p);
eul = rotm2eul(T(1:3,1:3),'XYZ');
th3 = norm(p)+pOffset; %because S3 is defined as negative
thetaList = [[eul(1) eul(2)] th3];
% simplify Angle, maybe not needed
for g = 1:1:length(thetaList)
if thetaList(g)>pi
thetaList(g) = thetaList(g)-2*pi; end
if thetaList(g)<-pi
thetaList(g) = thetaList(g)+2*pi; end
end
% Forward Kinematics to Check Validity
thetaMesh(i,:) = thetaList(1:3);
end
end
get Sphere Mesh
function mesh = getSphereMesh(location,radius,n)
%Inputs: radius in meters, location as a 3x1 vector in meters
% note that meshes are plotted with with the surf object
[X,Y,Z] = sphere(n); % nominal sphere radius 1, location 0,0,0
 
%Scale the sphere to the radius specified
X = X * radius; Y = Y * radius; Z = Z * radius;
 
%Translate the sphere to the location specificied
X = X + location(1); Y = Y + location(2); Z = Z + location(3);
 
%reshape the mesh
vertexNumber = length(X(1,:));
Xlist = reshape(X,[vertexNumber^2,1]);
Ylist = reshape(Y,[vertexNumber^2,1]);
Zlist = reshape(Z,[vertexNumber^2,1]);
mesh = [Xlist Ylist Zlist];
end
Plot Mesh and figure settings
function fig = plotMesh(mesh)
fig = scatter3(mesh(:,1),mesh(:,2),mesh(:,3)); plotSetting(fig);
end
function plotSetting(fig)
richBlackHex = '#080C0C'; white = '#F6F5F5';
ax = gca; % Get handle to current axes.
% Grid
ax. GridAlpha = 0.03; % Make grid lines less transparent.
ax. GridColor = richBlackHex;
ax.Box = 'off';
% Axis
ax. LineWidth = 1.5;
ax. YAxis.LineWidth = 4; ax.XAxis.LineWidth = 4; ax.ZAxis.LineWidth = 4;
ax.XColor = richBlackHex; % Red.
ax.YColor = richBlackHex; % Blue.
axis equal
% Figure
fig.MarkerEdgeColor = richBlackHex;
fig.MarkerFaceColor = richBlackHex;
%Labels
end
Forward Kinematics Custom Call
function Tnow = customForwardKinematics(Mlist_joints,Slist,n,theta_now)
%Reformatting inputs for compatibility with Modern Robotics code
M = eye(4);
for i = 1:1:n
M = Mlist_joints(:,:,n) * M; %M is then equal to the M0n
end
S = Slist(:,1:n); %Slist from joint 1 to n
theta = theta_now(1:n); %theta from joint 1 to n
Tnow = FKinSpace(M,S,theta); %the current joints transformation
end
dVRK Architecture
function [Slist Mlist_joints] = getdVRKArchitecture()
%meters
S1 = [1 0 0 0 0 0]; % joint 1 rotates about the x axis
S2 = [0 1 0 0 0 0]; % joint 2 rotates about the y axis
S3 = [0 0 0 0 0 -1]; % joint 3 translates in the -z axis
S4 = [0 0 -1 0 0 0]; % joint 4 rotates about the z axis
S5 = [0 1 0 -0.0091 0 0]; %S5 = [0 1 0 -0.91 0 0];
S6 = [1 0 0 0 0 0];
Slist = [S1' S2' S3' S4' S5' S6'];
 
%MLIST JOINTS
M01 = [eye(3) [0; 0; 0]; zeros(1,3) 1];
M12 = [eye(3) [0; 0; 0]; zeros(1,3) 1];
M23 = [eye(3) [0; 0; 0]; zeros(1,3) 1];
M34 = [eye(3) [0; 0; 0]; zeros(1,3) 1];
M45 = [eye(3) [0; 0; -0.0091]; zeros(1,3) 1];
M56 = [eye(3) [0; 0; 0]; zeros(1,3) 1];
Mlist_joints = cat(3,M01,M12,M23,M34,M45,M56);
end

Modern Robotics Textbook Functions

function [thetalist, success] ...
= IKinSpace(Slist, M, T, thetalist0, eomg, ev)
% *** CHAPTER 6: INVERSE KINEMATICS ***
% Takes Slist: The joint screw axes in the space frame when the manipulator
% is at the home position, in the format of a matrix with the
% screw axes as the columns,
% M: The home configuration of the end-effector,
% T: The desired end-effector configuration Tsd,
% thetalist0: An initial guess of joint angles that are close to
% satisfying Tsd,
% eomg: A small positive tolerance on the end-effector orientation
% error. The returned joint angles must give an end-effector
% orientation error less than eomg,
% ev: A small positive tolerance on the end-effector linear position
% error. The returned joint angles must give an end-effector
% position error less than ev.
% Returns thetalist: Joint angles that achieve T within the specified
% tolerances,
% success: A logical value where TRUE means that the function found
% a solution and FALSE means that it ran through the set
% number of maximum iterations without finding a solution
% within the tolerances eomg and ev.
% Uses an iterative Newton-Raphson root-finding method.
% The maximum number of iterations before the algorithm is terminated has
% been hardcoded in as a variable called maxiterations. It is set to 20 at
% the start of the function, but can be changed if needed.
thetalist = thetalist0;
i = 0;
maxiterations = 20;
Tsb = FKinSpace(M, Slist, thetalist);
Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T));
err = norm(Vs(1: 3)) > eomg || norm(Vs(4: 6)) > ev;
while err && i < maxiterations
thetalist = thetalist + pinv(JacobianSpace(Slist, thetalist)) * Vs;
i = i + 1;
Tsb = FKinSpace(M, Slist, thetalist);
Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T));
err = norm(Vs(1: 3)) > eomg || norm(Vs(4: 6)) > ev;
end
success = ~ err;
end
function T = FKinSpace(M, Slist, thetalist)
% *** CHAPTER 4: FORWARD KINEMATICS ***
% Takes M: the home configuration (position and orientation) of the
% end-effector,
% Slist: The joint screw axes in the space frame when the manipulator
% is at the home position,
% thetalist: A list of joint coordinates.
% Returns T in SE(3) representing the end-effector frame, when the joints
% are at the specified coordinates (i.t.o Space Frame).
T = M;
for i = size(thetalist): -1: 1
T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
end
function se3mat = VecTose3(V)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 6-vector (representing a spatial velocity).
% Returns the corresponding 4x4 se(3) matrix.
se3mat = [VecToso3(V(1: 3)), V(4: 6); 0, 0, 0, 0];
end
end
function se3mat = VecTose3(V)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 6-vector (representing a spatial velocity).
% Returns the corresponding 4x4 se(3) matrix.
se3mat = [VecToso3(V(1: 3)), V(4: 6); 0, 0, 0, 0];
end
function omg = so3ToVec(so3mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 3x3 skew-symmetric matrix (an element of so(3)).
% Returns the corresponding 3-vector (angular velocity).
omg = [so3mat(3, 2); so3mat(1, 3); so3mat(2, 1)];
end
function so3mat = VecToso3(omg)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 3-vector (angular velocity).
% Returns the skew symmetric matrix in so(3).
so3mat = [0, -omg(3), omg(2); omg(3), 0, -omg(1); -omg(2), omg(1), 0];
end
function [omghat, theta] = AxisAng3(expc3)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes A 3-vector of exponential coordinates for rotation.
% Returns the unit rotation axis omghat and the corresponding rotation
% angle theta.
theta = norm(expc3);
omghat = expc3 / theta;
end
function R = MatrixExp3(so3mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 3x3 so(3) representation of exponential coordinates.
% Returns R in SO(3) that is achieved by rotating about omghat by theta
% from an initial orientation R = I.
omgtheta = so3ToVec(so3mat);
if NearZero(norm(omgtheta))
R = eye(3);
else
[omghat, theta] = AxisAng3(omgtheta);
omgmat = so3mat / theta;
R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat;
end
end
function T = MatrixExp6(se3mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a se(3) representation of exponential coordinates.
% Returns a T matrix in SE(3) that is achieved by traveling along/about the
% screw axis S for a distance theta from an initial configuration T = I.
omgtheta = so3ToVec(se3mat(1: 3, 1: 3));
if NearZero(norm(omgtheta))
T = [eye(3), se3mat(1: 3, 4); 0, 0, 0, 1];
else
[omghat, theta] = AxisAng3(omgtheta);
omgmat = se3mat(1: 3, 1: 3) / theta;
T = [MatrixExp3(se3mat(1: 3, 1: 3)), ...
(eye(3) * theta + (1 - cos(theta)) * omgmat ...
+ (theta - sin(theta)) * omgmat * omgmat) ...
* se3mat(1: 3, 4) / theta;
0, 0, 0, 1];
end
end
function V = se3ToVec(se3mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes se3mat a 4x4 se(3) matrix
% Returns the corresponding 6-vector (representing spatial velocity).
V = [se3mat(3, 2); se3mat(1, 3); se3mat(2, 1); se3mat(1: 3, 4)];
end
function AdT = Adjoint(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes T a transformation matrix SE3.
% Returns the corresponding 6x6 adjoint representation [AdT].
[R, p] = TransToRp(T);
AdT = [R, zeros(3); VecToso3(p) * R, R];
end
function [R, p] = TransToRp(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes the transformation matrix T in SE(3)
% Returns R: the corresponding rotation matrix
% p: the corresponding position vector .
R = T(1: 3, 1: 3);
p = T(1: 3, 4);
end
function invT = TransInv(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a transformation matrix T.
% Returns its inverse.
[R, p] = TransToRp(T);
invT = [transpose(R), -transpose(R) * p; 0, 0, 0, 1];
end
function expmat = MatrixLog6(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a transformation matrix T in SE(3).
% Returns the corresponding se(3) representation of exponential
% coordinates.
[R, p] = TransToRp(T);
omgmat = MatrixLog3(R);
if isequal(omgmat, zeros(3))
expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0];
else
theta = acos((trace(R) - 1) / 2);
expmat = [omgmat, (eye(3) - omgmat / 2 ...
+ (1 / theta - cot(theta / 2) / 2) ...
* omgmat * omgmat / theta) * p;
0, 0, 0, 0];
end
end
function so3mat = MatrixLog3(R)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes R (rotation matrix).
% Returns the corresponding so(3) representation of exponential
% coordinates.
acosinput = (trace(R) - 1) / 2;
if acosinput >= 1
so3mat = zeros(3);
elseif acosinput <= -1
if ~NearZero(1 + R(3, 3))
omg = (1 / sqrt(2 * (1 + R(3, 3)))) ...
* [R(1, 3); R(2, 3); 1 + R(3, 3)];
elseif ~NearZero(1 + R(2, 2))
omg = (1 / sqrt(2 * (1 + R(2, 2)))) ...
* [R(1, 2); 1 + R(2, 2); R(3, 2)];
else
omg = (1 / sqrt(2 * (1 + R(1, 1)))) ...
* [1 + R(1, 1); R(2, 1); R(3, 1)];
end
so3mat = VecToso3(pi * omg);
else
theta = acos(acosinput);
so3mat = theta * (1 / (2 * sin(theta))) * (R - R');
end
end
function Js = JacobianSpace(Slist, thetalist)
% *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS ***
% Takes Slist: The joint screw axes in the space frame when the manipulator
% is at the home position, in the format of a matrix with the
% screw axes as the columns,
% thetalist: A list of joint coordinates.
% Returns the corresponding space Jacobian (6xn real numbers).
Js = Slist;
T = eye(4);
for i = 2: length(thetalist)
T = T * MatrixExp6(VecTose3(Slist(:, i - 1) * thetalist(i - 1)));
Js(:, i) = Adjoint(T) * Slist(:, i);
end
end
function traj = JointTrajectory(thetastart, thetaend, Tf, N, method)
% *** CHAPTER 9: TRAJECTORY GENERATION ***
% Takes thetastart: The initial joint variables,
% thetaend: The final joint variables,
% Tf: Total time of the motion in seconds from rest to rest,
% N: The number of points N > 1 (Start and stop) in the discrete
% representation of the trajectory,
% method: The time-scaling method, where 3 indicates cubic
% (third-order polynomial) time scaling and 5 indicates
% quintic (fifth-order polynomial) time scaling.
% Returns traj: A trajectory as an N x n matrix, where each row is an
% n-vector of joint variables at an instant in time. The
% first row is thetastart and the Nth row is thetaend . The
% elapsed time between each row is Tf/(N - 1).
% The returned trajectory is a straight-line motion in joint space.
timegap = Tf / (N - 1);
traj = zeros(size(thetastart, 1), N);
for i = 1: N
if method == 3
s = CubicTimeScaling(Tf, timegap * (i - 1));
else
s = QuinticTimeScaling(Tf, timegap * (i - 1));
end
traj(:, i) = thetastart + s * (thetaend - thetastart);
end
traj = traj';
end
function s = QuinticTimeScaling(Tf, t)
% *** CHAPTER 9: TRAJECTORY GENERATION ***
% Takes Tf: Total time of the motion in seconds from rest to rest,
% t: The current time t satisfying 0 < t < Tf.
% Returns s: The path parameter s(t) corresponding to a fifth-order
% polynomial motion that begins and ends at zero velocity and
% zero acceleration.
s = 10 * (t / Tf) ^ 3 - 15 * (t / Tf) ^ 4 + 6 * (t / Tf) ^ 5;
end
function taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, ...
g, Ftip, Mlist, Glist, Slist)
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
% Takes thetalist: n-vector of joint variables,
% dthetalist: n-vector of joint rates,
% ddthetalist: n-vector of joint accelerations,
% g: Gravity vector g,
% Ftip: Spatial force applied by the end-effector expressed in frame
% {n+1},
% Mlist: List of link frames {i} relative to {i-1} at the home
% position,
% Glist: Spatial inertia matrices Gi of the links,
% Slist: Screw axes Si of the joints in a space frame, in the format
% of a matrix with the screw axes as the columns.
% Returns taulist: The n-vector of required joint forces/torques.
% This function uses forward-backward Newton-Euler iterations to solve the
% equation:
% taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...
% + g(thetalist) + Jtr(thetalist) * Ftip
n = size(thetalist, 1);
Mi = eye(4);
Ai = zeros(6, n);
AdTi = zeros(6, 6, n + 1);
Vi = zeros(6, n + 1);
Vdi = zeros(6, n + 1);
Vdi(4: 6, 1) = -g;
AdTi(:, :, n + 1) = Adjoint(TransInv(Mlist(:, :, n + 1)));
Fi = Ftip;
taulist = zeros(n, 1);
for i=1: n
Mi = Mi * Mlist(:, :, i);
Ai(:, i) = Adjoint(TransInv(Mi)) * Slist(:, i);
AdTi(:, :, i) = Adjoint(MatrixExp6(VecTose3(Ai(:, i) ...
* -thetalist(i))) * TransInv(Mlist(:, :, i)));
Vi(:, i + 1) = AdTi(:, :, i) * Vi(:, i) + Ai(:, i) * dthetalist(i);
Vdi(:, i + 1) = AdTi(:, :, i) * Vdi(:, i) ...
+ Ai(:, i) * ddthetalist(i) ...
+ ad(Vi(:, i + 1)) * Ai(:, i) * dthetalist(i);
end
for i = n: -1: 1
Fi = AdTi(:, :, i + 1)' * Fi + Glist(:, :, i) * Vdi(:, i + 1) ...
- ad(Vi(:, i + 1))' * (Glist(:, :, i) * Vi(:, i + 1));
taulist(i) = Fi' * Ai(:, i);
end
end
function adV = ad(V)
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
% Takes V: 6-vector spatial velocity.
% Returns adV: The corresponding 6x6 matrix.
% Used to calculate the Lie bracket [V1, V2] = [adV1]V2
omgmat = VecToso3(V(1: 3));
adV = [omgmat, zeros(3); VecToso3(V(4: 6)), omgmat];
end
function judge = NearZero(near)
% *** BASIC HELPER FUNCTIONS ***
% Takes a scalar.
% Checks if the scalar is small enough to be neglected.
% Example Input:
%
% clear; clc;
% near = -1e-7;
% judge = NearZero(near)
%
% Output:
% judge =
% 1
judge = norm(near) < 1e-6;
end